/** 
 * yunTai.c - The C source file of the task
 * NOTE: This file is based on C99 and rt-thread
 *      该文件实现的任务有：
 *      - 云台控制伺服
 *      - 激光控制伺服
 *
 * Copyright (c) 2020-, magnetic Emit Project
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author            Notes           mail
 * 2021-06-15     江榕煜       first version   2373180028@qq.com
**/

#include "system.h"
#include "math.h"

float pitch_Pos=0,yaw_Pos=-100;

ALIGN(RT_ALIGN_SIZE)
static struct rt_thread thread_yunTai;  // 线程静态内存
static char stack_yunTai[1024];

struct rt_semaphore sem_YuntaiServer;    // 信号量静态内存

/*----------------------具体实现---------------------*/

/** @brief  云台pitch角与重心偏移关系
 *  @note   使用三角级数
 *  @author 江榕煜（2021.6.17）
 *  @param 
 *      duty(float)     pitch轴占空比
 *  @retval
 *      bias(int32_t)   重心偏移量
**/
int32_t pitch2BiasModel(float duty)
{
    //return 0.7143*duty+585;
    return 675.2*sin(0.0003814*duty -4.324) + 47.83*sin(0.06698*duty +2.667);
}


/** @brief  云台控制服务
 *  @note   使用邮箱触发控制
 *  @author 江榕煜（2021.6.15）
 *  @param 
 *  @retval
**/
static void thread_yunTai_Entry(void* entryParam)
{
    extern int32_t angle_bias;
    for(;;)
    {
        rt_sem_take(&sem_YuntaiServer,RT_WAITING_FOREVER);  // 永远等待获取信号量
        
        steerEngine_SetPos(0,pitch_Pos);    // 位置设置
        steerEngine_SetPos(1,yaw_Pos);
        
        angle_bias = pitch2BiasModel(yaw_Pos);  // 有点尴尬，命名写反了T_T
    }
}

/** @brief  云台控制服务初始化
 *  @note   
 *  @author 江榕煜（2021.6.15）
 *  @param  None
 *  @retval
 *          sem_yunTai_t(rt_sem_t)  信号量对象
**/
rt_sem_t thread_yunTai_Init(void)
{
    /* 信号量初始化 */
    rt_sem_init(&sem_YuntaiServer,"yunTai",0,RT_IPC_FLAG_FIFO);
    
    /* 静态线程初始化 */
    rt_thread_init(&thread_yunTai,
                    "yunTai thread",
                    thread_yunTai_Entry,
                    RT_NULL,
                    &stack_yunTai[0],
                    sizeof(stack_yunTai),
                    10,10);
                    
    
    rt_thread_startup(&thread_yunTai);  //启动线程
    rt_kprintf("yunTai-thread start success!\n");
                    
    return &sem_YuntaiServer;
}

void yunTai(int argn,char ** argv)
{
    switch(argv[1][0])
    {
        case('u'):
            yaw_Pos += 10;
                    
            if(yaw_Pos > 100) yaw_Pos = 100;
            else if(yaw_Pos < -100) yaw_Pos = -100;
            
            rt_sem_release(&sem_YuntaiServer);
            return;
        case('d'):
            yaw_Pos -= 10;
                    
            if(yaw_Pos > 100) yaw_Pos = 100;
            else if(yaw_Pos < -100) yaw_Pos = -100;
            
            rt_sem_release(&sem_YuntaiServer);
            return;
    }
}
MSH_CMD_EXPORT(yunTai,云台调整);
